#include "pid.h"
#include "stdint-gcc.h"
#include "math.h"



void set_pid(p_motor_pid pid, float kp, float ki, float kd, float max)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->out_max = max;
}

void pid_ctrl(p_motor_pid pid, float input_now, float tar)
{

    pid->pre = input_now;
    pid->tar =  tar;
    pid->err = pid->tar - pid->pre;
    //set dead zone of current at closed loop
    if((pid->err > - PID_DEAD_ZONE) && (pid->err < PID_DEAD_ZONE))
    {
        pid->err = 0.0f;
        pid->integral  = 0.0f;
        pid->err_last = 0.0f;
    }

    if(pid->err > -PID_INTERG_START_ERROR && pid->err < PID_INTERG_START_ERROR)
    {
        pid->integral += pid->err;
        if(pid->integral > PID_INTERG_MAX_VAL)
        {
            pid->integral = PID_INTERG_MAX_VAL;
        }
        else if(pid->integral < -PID_INTERG_MAX_VAL)
        {
            pid->integral = -PID_INTERG_MAX_VAL;
        }
    }

    pid->out = pid->kp * pid->err \
             + pid->ki * pid->integral \
             + pid->kd * (pid->err - pid->err_last);
    pid->err_last = pid->err;
}

